#!/usr/bin/python
# -*- coding: UTF-8 -*-
import time
import numpy as np

def msSleep(ms):
    time.sleep(0.001 * ms)

class ClimberRobot:     
    m = 1
    n = 1
    a = 1  #攀爬机器人前后运动的一些相关标志位（具体逻辑不用管，照着用就行了）
    _initAction = [50, -90, -90, 50]   #G T T G
    _upActions = np.array([[5,-90, -90, 5],\
                           [50,-90, -90, 5],\
                           [50, 0, 0, 5],\
                           [50, 90, 90, 5],\
                           [5, 90, 90, 5],\
                           [5, 90, 90, 5],\
                           [5, 90, 90, 50],\
                           [5, 90, 90, 50],\
                           [5, -90,-90, 50],\
                           [5, -90, -90, 5]])
    _downActions = np.array([[5, -90, -90, 5],\
                             [5, -90, -90, 50],\
                             [5, 0, 0, 50],\
                             [5, 90, 90, 50],\
                             [5, 90, 90, 5],\
                             [5, 90, 90, 5], \
                             [50, 90, 90, 5],\
                             [50, 0, 0, 5],\
                             [50, -90, -90, 5],\
                             [5, -90, -90, 5]])

    #ID 列表 G T T G
    _ids = [0] * 4

    #初始化机器人：使用 BwRobotLib 对象
    def __init__(self,mclib):
        self.lib = self.mclib

    def bindIds(self,ids):
       self._ids[0] = ids[1]
       self._ids[1] = ids[0]
       self._ids[2] = ids[2]
       self._ids[3] = ids[3]

    def moveToAction(self,actions):
        self.lib.SetMotorPositionSync(self._ids, actions)
        pass

    def runGrip(self):
        print("闭合夹子")
        #ID
        ids = [self._ids[0],self._ids[3]]
        angles = [6 ,6]
        self.lib.SetMotorPositionSync(ids, angles)
        pass 

    def runRelease(self):
        print("打开夹子")
        #ID
        ids = [self._ids[0],self._ids[3]]
        angles = [50 ,50]
        self.lib.SetMotorPositionSync(ids, angles)
        pass 

    def goForward(self):
        print("往前走")
        if(self.a == 1):
            self.m = 1
        if(self.a == 2):
            self.m = 2

        if(self.m == 1):
            self.a = 2
            self.n = 2
            self.moveToAction(self._upActions[0])
            msSleep(300)   #休息0.3秒
            self.moveToAction(self._upActions[1])
            msSleep(1500)   #休息1.5秒
            self.moveToAction(self._upActions[2])
            msSleep(300)   #休息0.3秒
            self.moveToAction(self._upActions[3])
            msSleep(1750)   #休息1.75秒
            self.moveToAction(self._upActions[4])
        if(self.m == 2):
            self.a = 1
            self.n = 1
            self.moveToAction(self._upActions[5])
            msSleep(300)   #休息0.3秒
            self.moveToAction(self._upActions[6])
            msSleep(1500)   #休息1.5秒
            self.moveToAction(self._upActions[7])
            msSleep(300)   #休息0.3秒
            self.moveToAction(self._upActions[8])
            msSleep(1750)   #休息1.75秒
            self.moveToAction(self._upActions[9])
        pass

    def goBack(self):
        print("往后走")
        if(self.n == 1):
            self.m = 1
        if(self.n == 2):
            self.m = 2

        if(self.m == 1):
            self.a = 2
            self.n = 2
            self.moveToAction(self._downActions[0])
            msSleep(300)   #休息0.3秒
            self.moveToAction(self._downActions[1])
            msSleep(1500)   #休息1.5秒
            self.moveToAction(self._downActions[2])
            msSleep(300)   #休息0.3秒
            self.moveToAction(self._downActions[3])
            msSleep(1750)   #休息1.75秒
            self.moveToAction(self._downActions[4])
        if(self.m == 2):
            self.a = 1
            self.n = 1
            self.moveToAction(self._downActions[5])
            msSleep(300)   #休息0.3秒
            self.moveToAction(self._downActions[6])
            msSleep(1500)   #休息1.5秒
            self.moveToAction(self._downActions[7])
            msSleep(300)   #休息0.3秒
            self.moveToAction(self._downActions[8])
            msSleep(1750)   #休息1.75秒
            self.moveToAction(self._downActions[9])
        pass
